/*
 * bt_publisher.cpp
 *
 *  Created on: Nov 4, 2011
 *      Author: Tomonori Yamamoto
 */

#include <ros/ros.h>
#include <cmath>
#include "biotac/bt_publisher.h"
#include <sr_robot_msgs/BiotacAll.h>
#include <sr_robot_msgs/Biotac.h>

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "bt_publisher");
  ros::NodeHandle n;

  /**
   * The advertise() function is how you tell ROS that you want to
   * publish on a given topic name. This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing. After this advertise() call is made, the master
   * node will notify anyone who is trying to subscribe to this topic name,
   * and they will in turn negotiate a peer-to-peer connection with this
   * node.  advertise() returns a Publisher object which allows you to
   * publish messages on that topic through a call to publish().  Once
   * all copies of the returned Publisher object are destroyed, the topic
   * will be automatically unadvertised.
   *
   * The second parameter to advertise() is the size of the message queue
   * used for publishing messages.  If messages are published more quickly
   * than we can send them, the number here specifies how many messages to
   * buffer up before throwing some away.
   */

  // ros::Publisher bt_pub[5];
  ros::Publisher bt_pub;
  std::string bt_prefix = "biotac/bt";

  // for (int i=0; i<5; i++)
  // {
  // 	  std::string bt_number = boost::lexical_cast<std::string>(i+1);
  // 	  std::string bt_topicname = bt_prefix + bt_number;
  // 	  bt_pub[i] = n.advertise<sr_robot_msgs::BiotacAll>(bt_topicname, 5);
  // }
  
  bt_pub = n.advertise<sr_robot_msgs::BiotacAll>("/biotac", 5);

  // update rate of 100Hz
  ros::Rate loop_rate(100);

  /**
   * A count of how many messages we have sent. This is used to create
   * a unique string for each message.
   */
  int count = 0;
  sr_robot_msgs::BiotacAll bt;

  int amp = 2047;
  int offset = 2048;
  int freq[19] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19};
  const int PI = 3.1415926;

  while (ros::ok())
  {
    /**
     * This is a message object. You stuff it with data, and then publish it.
     */
    bt.tactiles[0].pac0 = amp * sin(0.001*PI*freq[0]*count) + offset;
    bt.tactiles[0].pac1 = amp * sin(0.001*PI*freq[0]*count) + offset;
    bt.tactiles[0].pdc = amp * sin(0.001*PI*freq[0]*count) + offset;
    bt.tactiles[0].tac = amp * sin(0.001*PI*freq[0]*count) + offset;
    bt.tactiles[0].tdc = amp * sin(0.001*PI*freq[0]*count) + offset;
    bt.tactiles[0].electrodes[0] = amp * sin(0.001*PI*freq[0]*count) + offset;
    bt.tactiles[0].electrodes[1] = amp * sin(0.001*PI*freq[1]*count) + offset;
    bt.tactiles[0].electrodes[2] = amp * sin(0.001*PI*freq[2]*count) + offset;
    bt.tactiles[0].electrodes[3] = amp * sin(0.001*PI*freq[3]*count) + offset;
    bt.tactiles[0].electrodes[4] = amp * sin(0.001*PI*freq[4]*count) + offset;
    bt.tactiles[0].electrodes[5] = amp * sin(0.001*PI*freq[5]*count) + offset;
    bt.tactiles[0].electrodes[6] = amp * sin(0.001*PI*freq[6]*count) + offset;
    bt.tactiles[0].electrodes[7] = amp * sin(0.001*PI*freq[7]*count) + offset;
    bt.tactiles[0].electrodes[8] = amp * sin(0.001*PI*freq[8]*count) + offset;
    bt.tactiles[0].electrodes[9] = amp * sin(0.001*PI*freq[9]*count) + offset;
    bt.tactiles[0].electrodes[10] = amp * sin(0.001*PI*freq[10]*count) + offset;
    bt.tactiles[0].electrodes[11] = amp * sin(0.001*PI*freq[11]*count) + offset;
    bt.tactiles[0].electrodes[12] = amp * sin(0.001*PI*freq[12]*count) + offset;
    bt.tactiles[0].electrodes[13] = amp * sin(0.001*PI*freq[13]*count) + offset;
    bt.tactiles[0].electrodes[14] = amp * sin(0.001*PI*freq[14]*count) + offset;
    bt.tactiles[0].electrodes[15] = amp * sin(0.001*PI*freq[15]*count) + offset;
    bt.tactiles[0].electrodes[16] = amp * sin(0.001*PI*freq[16]*count) + offset;
    bt.tactiles[0].electrodes[17] = amp * sin(0.001*PI*freq[17]*count) + offset;
    bt.tactiles[0].electrodes[18] = amp * sin(0.001*PI*freq[18]*count) + offset;

//    ROS_INFO("%s", msg.data.c_str());

    /**
     * The publish() function is how you send messages. The parameter
     * is the message object. The type of this object must agree with the type
     * given as a template parameter to the advertise<>() call, as was done
     * in the constructor above.
     */
    bt_pub.publish(bt);

    ros::spinOnce();
    
    loop_rate.sleep();
    ++count;
  }


  return 0;
}
